#include<stdio.h>
#include <math.h>
#include<stm32f10x.h>
#include "LSM303DLH.h"
#include "Acc_Calc.h"
#include <math.h>
#include "stm32f10x_rcc.h"




#define DELAY_COUNT    0xFFFFF


RCC_ClocksTypeDef RCC_ClockFreq;
ErrorStatus HSEStartUpStatus;


// Declare globally to see values on debug watch window at all times.
double angles[3];
s16 acc_data_array[3][FILTSIZE];
s16 acc_data[3];
s16 acc_filtered[3][FILTSIZE];
s16 acc_final[3];
double atanTable[ATAN_SIZE];
void SetSysClockTo72(void);



int main(void) {

	int i, j;

	LSM_Acc_ConfigTypeDef acc_conf;
	acc_conf.Power_Mode = LSM_Acc_Lowpower_NormalMode;	// Set Power Mode to normal to use the normal output rate
	acc_conf.ODR = LSM_Acc_ODR_50;				   		// 50 Hz output normal
	acc_conf.Axes_Enable = LSM_Acc_XYZEN;				// Enable XYZ axes
	acc_conf.FS = LSM_Acc_FS_2;							// 2 g full-scale for better precision
	acc_conf.Data_Update = LSM_Acc_BDU_Single;		   	// Single data output s.t. high and lows are changed 
														// at the same time as recommended
	acc_conf.Endianess = LSM_Acc_Big_Endian;	   		// Big Endian for simpler display of values
		 
	LSM303DLH_I2C_Init();						   		// Init I2C bus
	LSM303DLH_Acc_Reboot_Cmd();					   		// Reboot memory to make sure it is empty

	LSM303DLH_Acc_Config(&acc_conf);				   		// Configure Accelerometer with all selected values above
	
	SetSysClockTo72();
	i=0;
	atanGenerator(atanTable);
	i=1;
	//for (j = 0; j < 2500; j++) {  					//Testing loop for 2500 iterations
//	i = 0;
/*	while (1) {
		
	

		// Read data from I2C bus on the accelerometer
		LSM303DLH_Acc_Read_Acc(acc_data);		

		
		// Store data values in a 2D array
		acc_data_array[0][i] = acc_data[0];
		acc_data_array[1][i] = acc_data[1];
		acc_data_array[2][i] = acc_data[2];

		// Apply low pass filter
		lowpass_filter(acc_data_array[0], acc_filtered[0], i);
		lowpass_filter(acc_data_array[1], acc_filtered[1], i);
		lowpass_filter(acc_data_array[2], acc_filtered[2], i);

		// Filter values using recursive low-pass filter
		acc_final[0] = acc_filtered[0][i];
		acc_final[1] = acc_filtered[1][i];
		acc_final[2] = acc_filtered[2][i];
		


		lookup_gyro(acc_final, angles);			// Calculate gyroscope angles with a LUT
		//calculate_gyro(acc_final, angles);		
		
		//Reset index for the filter
		if (i >= FILTSIZE) {
		 	i = 0;
		} else {
		 	i++;
		}

	}
		 */
	return 0;
}

void SetSysClockTo72(void)
{
  /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------------------------*/   
  /* RCC system reset(for debug purpose) */
  RCC_DeInit();

  /* Enable HSE */
  RCC_HSEConfig(RCC_HSE_ON);

  /* Wait till HSE is ready */
  HSEStartUpStatus = RCC_WaitForHSEStartUp();

  if (HSEStartUpStatus == SUCCESS)
  {

 
    /* HCLK = SYSCLK */
    RCC_HCLKConfig(RCC_SYSCLK_Div1); 
  
    /* PCLK2 = HCLK */
    RCC_PCLK2Config(RCC_HCLK_Div1); 

    /* PCLK1 = HCLK/2 */
    RCC_PCLK1Config(RCC_HCLK_Div2);

    /* PLLCLK = 8MHz * 9 = 72 MHz */
    RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);

    /* Enable PLL */ 
    RCC_PLLCmd(ENABLE);

    /* Wait till PLL is ready */
    while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET)
    {
    }

    /* Select PLL as system clock source */
    RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
  }
}